import rclpy
from rclpy.node import Node
import math

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import Range

class FramePublisher(Node):

    def __init__(self):
        super().__init__('cranebot_frame_publisher')

        # Initialize the transform broadcaster
        self.arm_move = 0
        self.wire_move = 0
        self.publisher_1 = self.create_publisher(Range, '/cranebot/tof1', 10)
        self.publisher_2 = self.create_publisher(Range, '/cranebot/tof2', 10)
        self.subscription = self.create_subscription(
            Quaternion,
            '/cranebot/move',
            self.listener_callback,
            10
        )
        self.subscription
        self.operation_tag = "pause"
        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.handle_cranebot_pose)
        self.operation_start_time_tag = self.get_clock().now().nanoseconds
        self.operation_relay_time_tag = self.get_clock().now().nanoseconds

    def listener_callback(self, msg):
        if ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 0.0)):
            self.operation_tag = "pause"
            #print("PAUSE")
        elif ((msg.w == 0.0) & (msg.x == 1.0) & (msg.y == 0.0) & (msg.z == 0.0)):      
            self.operation_tag = "extend_forward"
            #print("extend_forward")
        elif ((msg.w == 0.0) & (msg.x == -1.0) & (msg.y == 0.0) & (msg.z == 0.0)):      
            self.operation_tag = "extend_back"
            #print("extend_back")
        elif ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 1.0)):    
            self.operation_tag = "lift_up"
            #print("lift_up")
        elif ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == -1.0)):     
            self.operation_tag = "lift_down"
            #print("lift_down")
        self.operation_start_time_tag = self.get_clock().now().nanoseconds
        

    def handle_cranebot_pose(self):
        self.operation_time_tag = self.get_clock().now().nanoseconds
        operation_period = (self.operation_time_tag-self.operation_relay_time_tag)/1000000000
        if self.operation_tag == "pause":
            pass
        elif self.operation_tag == "extend_forward":
            self.arm_move += operation_period * 1.295 /100
        elif self.operation_tag == "extend_back":
            if self.arm_move >= 0:
                self.arm_move -= operation_period * 1.295 /100
        elif self.operation_tag == "lift_up":
            self.wire_move -= operation_period * 1.78 /100
        elif self.operation_tag == "lift_down":
            self.wire_move += operation_period * 1.78 /100
        self.operation_relay_time_tag = self.operation_time_tag
        print(self.arm_move, self.wire_move)
        msg1 = Range()
        msg1.radiation_type = 2
        msg1.range = float(self.arm_move)
        self.publisher_1.publish(msg1)
        msg2 = Range()
        msg2.radiation_type = 2
        msg2.range = float(self.wire_move)
        self.publisher_2.publish(msg2)

def main():
    rclpy.init()
    node = FramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

if __name__ == '__main__':
    main()